#include "MatrixTools.h"

namespace math_tools
{
	namespace matrix_tools
	{
		void copyMatrix(const Matrix3D & matrix, Matrix3D & result)
		{
			for (int i = 0; i < 16; ++i)
			{
				result[i] = matrix[i];
			}
		}

		void identityMatrix(Matrix3D & matrix)
		{
			for (int i = 0; i < 16; ++i)
			{
				matrix[i] = (i % 5 == 0) ? 1.0 : 0.0;
			}
		}

		void multiplyMatrix(const Matrix3D & matrix1, const Matrix3D & matrix2, Matrix3D& result)
		{
			int numCols = 4;
			int numRows = 4;
			Matrix3D tmp;
			double * otherColJ = new double[numCols];
			for (int j = 0; j < numCols; j++)
			{
				for (int i = 0; i < numCols; i++)
				{
					otherColJ[i] = matrix1[i * numCols + j];
				}
				for (int i = 0; i < numRows; i++)
				{
					double sum = 0;
					for (int k = 0; k < numCols; k++)
					{
						sum += matrix2[i * numCols + k] * otherColJ[k];
					}
					tmp[i * numCols + j] = sum;
				}
			}
			copyMatrix(tmp,result);
			delete[] otherColJ;
		}

		void rotationMatrix(double angle, const Vector3D & vector, Matrix3D & rotation_matrix)
		{
			double c = cos(angle);
			double s = sin(angle);

			Vector3D axis = Vector3D::unit(vector);
			double x = axis.getX();
			double y = axis.getY();
			double z = axis.getZ();

			rotation_matrix[0] = sqr(x) + (1 - sqr(x)) * c;
			rotation_matrix[1] = x * y * (1 - c) + z * s;
			rotation_matrix[2] = x * z * (1 - c) - y * s;
			rotation_matrix[3] = 0.0;

			rotation_matrix[4] = x * y * (1 - c) - z * s;
			rotation_matrix[5] = sqr(y) + (1 - sqr(y)) * c;
			rotation_matrix[6] = y * z * (1 - c) + x * s;
			rotation_matrix[7] = 0.0;

			rotation_matrix[8] = x * z * (1 - c) + y * s;
			rotation_matrix[9] = y * z * (1 - c) - x * s;
			rotation_matrix[10] = sqr(z) + (1 - sqr(z)) * c;
			rotation_matrix[11] = 0.0;

			rotation_matrix[12] = 0.0;
			rotation_matrix[13] = 0.0;
			rotation_matrix[14] = 0.0;
			rotation_matrix[15] = 1.0;
		}

		void rotationMatrix(const Vector3D & vector, Matrix3D & rotation_matrix)
		{
			double angle = degToRad(vector.mag());
			rotationMatrix(angle, vector, rotation_matrix);
		}

		void transform(const Matrix3D & matrix, const Vector3D & vector, Vector3D & result)
		{
			double buff[4];
			for (int i = 0; i < 4; ++i)
			{
				buff[i] = vector.getX() * matrix[i * 4] + vector.getY() * matrix[1 + i * 4] + vector.getZ() * matrix[2 + i * 4] + matrix[3 + i * 4];
			}

			result.setX(buff[0]);
			result.setY(buff[1]);
			result.setZ(buff[2]);
		}

		void translationMatrix(double x, double y, double z, Matrix3D & translation_matrix)
		{
			translation_matrix[0] = 1.0;
			translation_matrix[1] = 0.0;
			translation_matrix[2] = 0.0;
			translation_matrix[3] = 0.0;

			translation_matrix[4] = 0.0;
			translation_matrix[5] = 1.0;
			translation_matrix[6] = 0.0;
			translation_matrix[7] = 0.0;

			translation_matrix[8] = 0.0;
			translation_matrix[9] = 0.0;
			translation_matrix[10] = 1.0;
			translation_matrix[11] = 0.0;

			translation_matrix[12] = x;
			translation_matrix[13] = y;
			translation_matrix[14] = z;
			translation_matrix[15] = 1.0;
		}

		void translationMatrix(const Vector3D & vector, Matrix3D & translation_matrix)
		{
			translationMatrix(vector.getX(), vector.getY(), vector.getZ(), translation_matrix);
		}
	}
}
